%电机轴是原点
alpha=175; %摆杆轴相对于电机轴角度，可调 90,95,100,105,110
e=60; %两轴距离mm，可调65
l1=35; %假定曲柄长mm32
l2=59; %假定摆筒长mm  48 

ef2=60; %前脚掌长（假定）
ef1=46; %脚后跟-脚踝距离，假设定值40
efy=24; %铰链在脚后跟上方垂直距离
sita=0:3:360; %曲柄转角

gama=20; %后撑杆上方固定杆和摆杆夹角
ls1=38; %后撑杆上方固定杆长
ls2=90; %后撑杆长

%lr=120;%活动支撑杆长

%% 
p=sqrt(e.*e+l1.*l1-2*e.*l1.*cosd((alpha-sita))); %摆杆上段长度
Cfai=(l1.*cosd(sita)-e.*cosd(alpha))./p;
Sfai=(l1.*sind(sita)-e.*sind(alpha))./p;
fai=atan2(Sfai,Cfai)/pi*180; 
V_e=[cosd(alpha);sind(alpha)];%两铰链方向（电机指向摆杆轴）
V_l2=[cosd(fai);sind(fai)]; %摆杆的绝对方向
V_lr=[cosd(fai-gama);sind(fai-gama)];%后撑杆上方固定杆方向

P_ankle=e.*V_e+(l2+p).*V_l2; %脚踝位置向量，第一行x_ankle，第二行y_ankle
% plot(P_ankle(1,:),P_ankle(2,:))

P_G=e*V_e+ls1*V_lr; %后撑杆上方铰链位置点坐标
%plot(P_G(1,:),P_G(2,:))
l_AG=sqrt((p+l2).^2+ls1.^2-2.*(p+l2).*ls1.*cosd(gama));
beta1=acosd(((p+l2).^2+l_AG.^2-ls1.^2)./(2.*(p+l2).*l_AG));
beta3=atand(efy/ef1);
l_AH=sqrt(ef1*ef1+efy*efy);
beta2=acosd((l_AH.^2+l_AG.^2-ls2.^2)./(2.*l_AH.*l_AG));
beta=beta1+beta2+beta3; %脚后跟与腿夹角

%beta=acosd(((p+l2).*(p+l2)+ef1*ef1-lr.*lr)./(2.*(p+l2).*ef1)); %脚后跟与腿夹角
V_f=[cosd(180+fai-beta);sind(180+fai-beta)];%脚面的绝对方向（脚后跟指向）
P_toe=P_ankle-ef2.*V_f; %脚尖位置向量，第一行x_toe，第二行y_toe
P_heel=P_ankle+ef1.*V_f;  %脚后跟位置向量，第一行x_heel，第二行y_heel
% P_heel=e.*V_e+lr.*V_l2; %脚后跟位置向量，第一行x_heel，第二行y_heel
% P_toe=P_heel-(ef1+ef2).*V_f;%脚尖位置向量，第一行x_heel，第二行y_heel
%plot(P_toe(1,:),P_toe(2,:))
%plot(P_heel(1,:),P_heel(2,:))

%% 画图
figure(1)
scatter(0,0)
hold on
plot(P_ankle(1,:),P_ankle(2,:))
plot(P_toe(1,:),P_toe(2,:))
plot(P_heel(1,:),P_heel(2,:))

%摆杆铰链-脚踝连线
xy_eToankle=[(e.*V_e).';(P_ankle).'];
xy_eAlink=zeros(length(sita)+1,length(sita)+1);
xy_eAlink(1,2:5:(length(sita)+1))=1;
xy_eAlink(2:5:(length(sita)+1),1)=1;
gplot(xy_eAlink,xy_eToankle,'b') 
%脚尖脚跟连线
xy_tToh=[(P_toe).';(P_heel).'];
xy_thlink=zeros(length(P_toe)+length(P_heel),length(P_toe)+length(P_heel));
for j=1:5:length(sita)
    xy_thlink(j,j+length(sita))=1;
    xy_thlink(j+length(sita),j)=1;
end
% xy_thlink(1:10:length(sita),(length(sita)+1):10:2*length(sita))=1;
% xy_thlink((length(sita)+1):10:2*length(sita),1:10:length(sita))=1;
gplot(xy_thlink,xy_tToh,'r')

hold off

figure(2)
plot(P_ankle(1,:),P_ankle(2,:),'o')
hold on
for j=1:max(size(sita))
    if rem(j,3)==0
        c = num2str(j*3);%数字转字符
        text(P_ankle(1,j),P_ankle(2,j),c);%在图上显示文字
    end
end
plot(P_toe(1,:),P_toe(2,:),'^')

%摆杆铰链-脚踝连线
xy_eToankle=[(e.*V_e).';(P_ankle).'];
xy_eAlink=zeros(length(sita)+1,length(sita)+1);
xy_eAlink(1,2:5:(length(sita)+1))=1;
xy_eAlink(2:5:(length(sita)+1),1)=1;
gplot(xy_eAlink,xy_eToankle,'b') 
%脚跟脚尖连线
xy_tToh=[(P_toe).';(P_heel).'];
xy_thlink=zeros(length(P_toe)+length(P_heel),length(P_toe)+length(P_heel));
for j=1:5:length(sita)
    xy_thlink(j,j+length(sita))=1;
    xy_thlink(j+length(sita),j)=1;
end
gplot(xy_thlink,xy_tToh,'r')
hold off